#include "imu.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "tf2/utils.h"

class ImuNode : public rclcpp::Node
{
  private:
	std::shared_ptr<Imu> _imu;
	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr _imu_pub;
	std::string _publish_topic = "imu_data";
	std::thread _process;
	std::string _interface_name = "can0";
	double _start_time = rclcpp::Clock().now().seconds();
	bool _self_calibrate = true;
	void process(void)
	{
		sensor_msgs::msg::Imu msg;
		msg.header.frame_id = "imu_link";
		msg.orientation_covariance.fill(0);
		msg.angular_velocity_covariance.fill(0);
		msg.linear_acceleration_covariance.fill(0);
		// see
		// https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py
		// self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
		// self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
		// self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0]
		msg.orientation_covariance = {1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6};
		msg.angular_velocity_covariance = {1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6};
		msg.linear_acceleration_covariance = {-1, 0, 0, 0, 0, 0, 0, 0, 0};
		int calibrating = 100;
		tf2::Transform offset;
		while (rclcpp::ok())
		{
			imu_data_t data;
			if (this->_imu->recv(data) == 0)
			{
				if (this->_self_calibrate)
				{
					if (calibrating > 0)
					{
						calibrating--; // wait for a second
					}
					else
					{
						// record initial orientation quad to correct orientation
						tf2::Quaternion q(data.quad_x, data.quad_y, data.quad_z, data.quad_w);
						offset = tf2::Transform(q);
						this->_self_calibrate = false; // end calibration
						RCLCPP_INFO(this->get_logger(),
									"calibration finished, offset x y z w: %lf %lf %lf %lf",
									data.quad_x, data.quad_y, data.quad_z, data.quad_w);
					}
				}
				else
				{
					msg.header.stamp = rclcpp::Clock().now();
					msg.angular_velocity.x = data.ang_x;
					msg.angular_velocity.y = data.ang_y;
					msg.angular_velocity.z = data.ang_z;
					msg.linear_acceleration.x = data.accel_x;
					msg.linear_acceleration.y = data.accel_y;
					msg.linear_acceleration.z = data.accel_z;
					tf2::Quaternion q(data.quad_x, data.quad_y, data.quad_z, data.quad_w);
					tf2::Transform tf(q);
					tf = tf * offset.inverse(); // correct
					msg.orientation.x = tf.getRotation().getX();
					msg.orientation.y = tf.getRotation().getY();
					msg.orientation.z = tf.getRotation().getZ();
					msg.orientation.w = tf.getRotation().getW();
					this->_imu_pub->publish(msg);
				}
			}
			else
			{
				RCLCPP_DEBUG(this->get_logger(), "recv timeout");
			}
		}
	}

  public:
	ImuNode(std::string name) : Node(name)
	{
		int _device_addr = 0;
		this->declare_parameter("publish_topic", _publish_topic);
		this->declare_parameter("interface_name", _interface_name);
		this->declare_parameter("device_addr", _device_addr);
		this->declare_parameter("self_calibrate", _self_calibrate);

		this->get_parameter("publish_topic", _publish_topic);
		this->get_parameter("interface_name", _interface_name);
		this->get_parameter("device_addr", _device_addr);
		this->get_parameter("self_calibrate", _self_calibrate);
		this->_imu = std::make_shared<Imu>(_device_addr);
		this->_imu->open(_interface_name);
		if (!this->_imu->is_open())
		{
			RCLCPP_ERROR(this->get_logger(), "failed to open interface, name: %s",
						 _interface_name.c_str());
			exit(EXIT_FAILURE);
		}
		RCLCPP_INFO(this->get_logger(), "open imu at interface %s, with device addr %d",
					_interface_name.c_str(), _device_addr);
		this->_imu_pub = this->create_publisher<sensor_msgs::msg::Imu>(_publish_topic, 100);
		if (this->_self_calibrate)
		{
			RCLCPP_INFO(this->get_logger(), "calibrating, please wait for a second");
		}
		_process = std::thread(std::bind(&ImuNode::process, this));
		_process.detach();
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<ImuNode>("imu_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
